#include"gene_traj/fast_planner.hpp"

namespace fast_planner 
{

/**
 * @brief 目标位置订阅函数
*/
// void FastPlan::GoalPose_Callback(const geometry_msgs::msg::PoseStamped::ConstPtr msg) 
MatrixXd FastPlan::MincoPlanManage( float cv_time, 
                                const Vector2d& Current_Pos,//现在位置 
                                const Vector2d& Current_Vel,//现在速度
                                const Vector2d& Current_Acc,//现在加速度
                                const Vector2d& Target_Pos, //目标位置
                                const Vector2d& Target_Vel, //目标速度
                                const Vector2d& Target_Acc  //目标加速度
                                                            )
{


    auto path_msg = nav_msgs::msg::Path();
  
    // vector<Vector2d> raw_path= {
    //                     /* 0*/  { 0.0, 0.0 },
    //                     /* 1*/  { 0.05, 0.05 },
    //                     /* 2*/  { 0.30, 0.30 },
    //                     /* 3*/  { 0.70, 0.70 },
    //                     /* 4*/  { 0.95, 0.95 },
    //                     /* 5*/  { 1.0, 1.0 },
    //                                             };
    //处理后规划的起点
    // begin_pose_ = raw_path[0];
    begin_pose_ = Current_Pos;//起点坐标
    begin_vel_  = Current_Vel;
    begin_acc_  = Current_Acc;
    cout<<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>"<<endl;
    plan_state<<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>"<<endl;

    //处理后规划的终点
    end_pose_ = Target_Pos;//终点坐标
    end_vel_  = Target_Vel;
    end_acc_  = Target_Acc;


    VectorXd time_vec;
    Matrix2Xd wayPs; //起点、中间点、终点
    Matrix2Xd innerWayPts;//中间点
    // wayPs.resize(2, raw_path.size());
    wayPs.resize(2, 5);
    seg_num_ = wayPs.cols()-1; //轨迹段数
    time_vec.resize(seg_num_);
    // for(int i = 0; i < raw_path.size(); i++)
    // {
    //     wayPs.col(i) = raw_path[i];
    // }

    float x_len, y_len;
    x_len = end_pose_(0) - begin_pose_(0);
    y_len = end_pose_(1) - begin_pose_(1);
    
    wayPs(0,0) = begin_pose_(0);   
    wayPs(0,1) = begin_pose_(0) + x_len * 0.18;
    wayPs(0,2) = begin_pose_(0) + x_len * 0.50;
    wayPs(0,3) = begin_pose_(0) + x_len * 0.82;
    wayPs(0,4) =   end_pose_(0);

        
    wayPs(1,0) = begin_pose_(1);    
    wayPs(1,1) = begin_pose_(1) + y_len * 0.18;
    wayPs(1,2) = begin_pose_(1) + y_len * 0.50;
    wayPs(1,3) = begin_pose_(1) + y_len * 0.82;
    wayPs(1,4) =   end_pose_(1);
  


    // cout<<"wayPs:"<<endl;
    // cout<<wayPs<<endl;

    innerWayPts = wayPs.block(0,1, 2,  seg_num_-1);//中间点
    //时间分配函数
    // TrapezoidAllocateTime(wayPs, time_vec, 6.0 , 3.7*2);
    TrapezoidAllocateTime(wayPs, time_vec, 4.0 , 3.5*2);

    // cout<<"***************************************************"<<endl;
    // cout<<"time_vec: "<<endl;
    // cout<<time_vec<<endl;
    float whole_time_vec = 0;
    for(int i = 0; i < time_vec.size(); i++)
    {
        whole_time_vec += time_vec(i);
    }
    // cout<<"whole time duration: "<<whole_time_vec<<" s"<<endl;


// auto node = std::make_shared<rclcpp::Node>("example1_node");
// rclcpp::Time time_start = node->now();    

    /************************生成MINCO**************************************************/
    poly_traj::MinJerkOpt best_MJO;
    getMincoTraj(best_MJO, time_vec, innerWayPts);

// rclcpp::Time time_end = node->now();
// RCLCPP_INFO(node->get_logger(),"生成轨迹所需的总时间是:%f ms", (time_end - time_start).seconds() * 1000);

    /*可视化*/
    poly_traj::Trajectory traj_ = best_MJO.getTraj();

    double step_size_t = 0.005; //每隔5ms取一个点
    double duration = traj_.getTotalDuration();//轨迹总时间

    // cout<<"duration:"<<duration<<endl;
    int i_end = floor(duration / step_size_t);
    MatrixXd TrajXY = MatrixXd::Zero(i_end + 1 , 8);

    for (int i = 0; i < TrajXY.rows() - 1; ++i) 
    {
        TrajXY(i, 0) =  traj_.getPos(i * step_size_t)(0);//px
        TrajXY(i, 1) =  traj_.getPos(i * step_size_t)(1);//py
        TrajXY(i, 2) =  traj_.getVel(i * step_size_t)(0);//vx
        TrajXY(i, 3) =  traj_.getVel(i * step_size_t)(1);//vy
        TrajXY(i, 4) =  traj_.getAcc(i * step_size_t)(0);//ax
        TrajXY(i, 5) =  traj_.getAcc(i * step_size_t)(1);//ay
        TrajXY(i, 6) =  traj_.getJer(i * step_size_t)(0);//jx
        TrajXY(i, 7) =  traj_.getJer(i * step_size_t)(1);//jy
    }
    //填充终点状态
    TrajXY(i_end, 0) = traj_.getPos(duration)(0);//px
    TrajXY(i_end, 1) = traj_.getPos(duration)(1);//py
    TrajXY(i_end, 2) = traj_.getVel(duration)(0);//vx;
    TrajXY(i_end, 3) = traj_.getVel(duration)(1);//vy;
    TrajXY(i_end, 4) = traj_.getAcc(duration)(0);//ax;
    TrajXY(i_end, 5) = traj_.getAcc(duration)(1);//ay;
    TrajXY(i_end, 6) = traj_.getJer(duration)(0);//jx;
    TrajXY(i_end, 7) = traj_.getJer(duration)(1);//jy;


    for(int i = 0; i < TrajXY.rows(); ++i)
    {

        minco_traj  << "t:" <<"\t"<< step_size_t * i <<"\t"<<
                            "x:" <<"\t"<<  TrajXY(i, 0) <<"\t"<< "y:"  <<"\t"<<  TrajXY(i, 1) <<"\t"<<
                            "vx:"<<"\t"<<  TrajXY(i, 2) <<"\t"<< "vy:" <<"\t"<<  TrajXY(i, 3) <<"\t"<< 
                            "ax:"<<"\t"<<  TrajXY(i, 4) <<"\t"<< "ay:" <<"\t"<<  TrajXY(i, 5) <<"\t"<< 
                            "jx:"<<"\t"<<  TrajXY(i, 6) <<"\t"<< "jy:" <<"\t"<<  TrajXY(i, 7) <<"\t"<< endl;

   }
minco_traj<<endl;
        

    cout<<"<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<"<<endl;
    cout<<endl;
    plan_state<<"<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<"<<endl;
    plan_state<<endl;

    return TrajXY;

}

    

    /**
     * @brief 中间路径点，每隔一定距离取一个点
     * @param path 路径集合
     * @param innerPoints 在路径集合，每隔0.1m取一个点传入该2行n列的矩阵
    */
   /*
    void FastPlan::getWayPoints(Matrix2Xd& innerPoints, nav_msgs::msg::Path& path) 
    {
        vector<Vector2d> wayPts;
        double whole_len = 0;//路径总长度
        double cur_len = 0; //当前路径的长度
        for (int i = 1; i < path.poses.size() - 1; ++i) 
        {
            Vector2d dist;
            dist(0) = path.poses[i].pose.position.x - path.poses[i - 1].pose.position.x;
            dist(1) = path.poses[i].pose.position.y - path.poses[i - 1].pose.position.y;
            cur_len += dist.norm();
            whole_len += dist.norm();

            if (cur_len >= seg_len_)  //当前路径从零累加，当前路径大于0.1m时，将此时的i计入轨迹，并将路径清零，以此循环直至结束。
            {
                cur_len = 0;
                Vector2d wayPt;
                wayPt(0) = path.poses[i].pose.position.x;
                wayPt(1) = path.poses[i].pose.position.y;

                if ((wayPt - end_pose_).norm() > 0.1)//如果当前点与目标点距离小于0.1就舍弃当前点
                    wayPts.emplace_back(wayPt);//(只有 当前点-目标点>0.1时，当前点才会被装入轨迹 )
            }

        }
        RCLCPP_INFO(this->get_logger(), "whole length = %f", whole_len);

        innerPoints.resize(2, wayPts.size());
        for (int i = 0; i < wayPts.size(); ++i) 
        {
            innerPoints.col(i) = wayPts[i];
        }
    }
    */



/**
 * @brief 时间分配
 * @param wayPs wayPs位置点
 * @param vel 最大速度
 * @param acc 最大加速度
*/
void FastPlan::TrapezoidAllocateTime(const MatrixXd &wayPs,VectorXd& time_vec, double vel, double acc)
{
    int N = (int)(wayPs.cols()) - 1;//轨迹段数
    //cout<<"N:"<<N<<endl;
    if (N > 0)
    {

        Vector3d p0, p1;

        //dtxyz 第k段轨迹时长   D   第k段轨迹的距离
        //acct  加速阶段时长  accd  加速阶段距离
        //dcct  减速阶段时长  dccd  减速阶段距离
        //t1, t2 加速时长 减速时长  （无匀速段时）
        //t1, t2, t3 加速时长 匀速时长 减速时长
        double dtxyz, D, acct, accd, dcct, dccd, t1, t2, t3;

        for (int k = 0; k < N; k++)
        {

            p0 << wayPs.col(k), 0;
            p1 << wayPs.col(k + 1), 0;

            D = (p1 - p0).norm();

            acct = vel / acc;               // 6 / 3.7 = 1.6 s   
            accd = (acc * acct * acct / 2); // 0.5 * 3.7 * 1.6^2 = 4.736 m    
            dcct = vel / acc;               // 1.6 s
            dccd = acc * dcct * dcct / 2;   // 4.736   

            if (D < accd + dccd)
            {
                t1 = sqrt(2 * D / acc);
                // t2 = (acc * t1) / acc;
                dtxyz = t1;
            }
            else
            {
                t1 = acct;
                t2 = (D - accd) / vel;
                // t3 = dcct;
                dtxyz = t1 + t2;
            }

            // if (D < accd + dccd)
            // {
            //     t1 = sqrt(acc * D) / acc;
            //     t2 = (acc * t1) / acc;
            //     dtxyz = t1 + t2;
            //     cout<<"in 1 : D < accd + dccd"<<endl;
            // }
            // else
            // {
            //     t1 = acct;
            //     t2 = (D - accd - dccd) / vel;
            //     t3 = dcct;
            //     dtxyz = t1 + t2 + t3;
            //     cout<<"in 1 : D >= accd + dccd"<<endl;
            // }

            time_vec(k) = dtxyz;
        }
        // cout<<"***************************************************"<<endl;
        // cout<< "acct: "<<acct <<endl;
        // cout<< "accd: "<<accd<<endl;

        // cout<< "dcct: "<<dcct <<endl;
        // cout<< "dccd: "<<dccd<<endl;
        // cout<< "accd + dccd: "<<accd + dccd<<endl;
        // cout<<"***************************************************"<<endl;
    }
}


void FastPlan::getMincoTraj(poly_traj::MinJerkOpt& best_MJO, const VectorXd& time_vec, const Matrix2Xd& innerPoints) 
{
//*********************************1、现获得不包含不等式约束的MINCO初值 即使用最优性充要条件，进行求解Mc = b*******************************************
    //获取初末状态，都转换成3d，z轴都设为0
    poly_traj::MinJerkOpt globalMJO;

    // 因为是minimum jerk，故是pva，三维

    //初始状态
    Matrix<double, 3, 3> headState;
    Vector3d start_pos = Vector3d::Zero();
    Vector3d start_vel = Vector3d::Zero();
    Vector3d start_acc = Vector3d::Zero();
    start_pos << begin_pose_, 0;
    start_vel << begin_vel_ , 0;
    start_acc << begin_acc_ , 0;
    headState << start_pos, start_vel, start_acc;

    cout <<"headState:"<<endl;
    cout <<headState.block(0, 0, 2, 3)<<endl;
    plan_state <<"headState:"<<endl;
    plan_state <<headState.block(0, 0, 2, 3)<<endl;
    

    //终止状态
    Matrix<double, 3, 3> tailState; // 初末状态
    Vector3d end_pos = Vector3d::Zero();
    Vector3d end_vel = Vector3d::Zero();
    Vector3d end_acc = Vector3d::Zero();
    end_pos << end_pose_, 0;
    end_vel << end_vel_ , 0;
    end_acc << end_acc_ , 0;
    tailState << end_pos, end_vel, end_acc;

    cout <<"tailState:"<<endl;
    cout <<tailState.block(0, 0, 2, 3)<<endl;
    plan_state <<"tailState:"<<endl;
    plan_state <<tailState.block(0, 0, 2, 3)<<endl;

    //获取3d中间点
    MatrixXd innerPts3d;
    int piece_num = time_vec.size();//轨迹段数
    innerPts3d.resize(3, piece_num - 1); //中间路径点
    for (int i = 0; i < piece_num - 1; ++i) 
    {
        Vector3d wayPt3d;
        wayPt3d << innerPoints.col(i), 0;
        innerPts3d.col(i) = wayPt3d;
    }

    //生成初始MINCO初值
    globalMJO.reset(headState, tailState, piece_num);
    globalMJO.generate(innerPts3d, time_vec);

//******************************2、加入不等式约束进行优化 即对minco轨迹进行时空联合优化，也就是轨迹变形********************************
    global_ploy_traj_opt_.reset(new ego_planner::PolyTrajOptimizer);
    global_ploy_traj_opt_ -> setParam(global_cps_num_prePiece_, wei_feas_, max_vel_, minco_max_acc_);//设置优化参数

    cout<<"Acc_Limit: "<< minco_max_acc_ << " m/s²" <<endl;
    plan_state<<"Acc_Limit: "<< minco_max_acc_ <<" m/s²"<<endl;
    //获得初始MINCO轨迹
    poly_traj::Trajectory initTraj = globalMJO.getTraj();
    double before_opt_duration = initTraj.getTotalDuration();


    int PN = initTraj.getPieceNum();//轨迹段数
    // TODO 应该传入 global_cps_num_prePiece_ 这个其实就是每一段轨迹分几个控制点来进行连续时间约束
    MatrixXd init_points = globalMJO.getInitConstraintPoints(global_cps_num_prePiece_);//初始控制点
    
    global_ploy_traj_opt_ -> global_setControlPoints(init_points);
    MatrixXd all_pos = initTraj.getPositions();
    MatrixXd innerWayPts = all_pos.block(0, 1, 3, PN - 1);
    headState << initTraj.getJuncPos(0), initTraj.getJuncVel(0), initTraj.getJuncAcc(0);
    tailState << initTraj.getJuncPos(PN), initTraj.getJuncVel(PN), initTraj.getJuncAcc(PN);
    
    double final_cost;
    bool flag_success = false; 
    // 最终的MINCO        
    flag_success = global_ploy_traj_opt_-> optimizeGlobalTrajectory(headState, tailState, innerWayPts, initTraj.getDurations(), final_cost);

    //获得优化后的MINCO轨迹
    best_MJO = global_ploy_traj_opt_->getMinJerkOpt();// 调用=运算符重载
    double after_opt_duration = best_MJO.getTraj().getTotalDuration();


printf("总时长  前:%.3f s,    后:%.3f s\n", before_opt_duration, after_opt_duration);
printf("MaxVel 前:%.3f m/s,  后:%.3f m/s\n", initTraj.getMaxVelRate(), best_MJO.getTraj().getMaxVelRate());
printf("MaxAcc 前:%.3f m/s², 后:%.3f m/s²\n", initTraj.getMaxAccRate(), best_MJO.getTraj().getMaxAccRate());

    plan_state << "总时长  前:"  << before_opt_duration     << " s  "<< "  后 :"  << after_opt_duration <<" s  "<< endl;
    plan_state << "MaxVel 前:" << initTraj.getMaxVelRate() <<" m/s " << "  后 : " << best_MJO.getTraj().getMaxVelRate() <<" m/s"<< endl;
    plan_state << "MaxAcc 前:" << initTraj.getMaxAccRate() <<" m/s²"<< "  后 : " << best_MJO.getTraj().getMaxAccRate() <<" m/s²"<< endl;


}



}


